#ifndef SERIAL_PORT_H
#define SERIAL_PORT_H

#include <fcntl.h>
#include <linux/serial.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/select.h>
#include <termios.h>
#include <unistd.h>

#include <array>
#include <chrono>
#include <iostream>
#include <memory>
#include <queue>
#include <string>

void print_data(const uint8_t* data, uint8_t len);

class SerialPort {
 public:
  using SharedPtr = std::shared_ptr<SerialPort>;

  SerialPort(std::string port, speed_t baudrate, int timeout_ms = 2) {
    set_timeout(timeout_ms);
    Init(port, baudrate);
  }

  ~SerialPort() { close(fd_); }

  ssize_t send(const uint8_t* data, size_t len) {
    // tcflush(fd_, TCIFLUSH);
    ssize_t ret = ::write(fd_, data, len);
    // tcdrain(fd_);
    return ret;
  }

  ssize_t recv(uint8_t* data, size_t len) {
    FD_ZERO(&rSet_);
    FD_SET(fd_, &rSet_);
    ssize_t recv_len = 0;

    switch (select(fd_ + 1, &rSet_, NULL, NULL, &timeout_)) {
      case -1:  // error
        // std::cout << "communication error" << std::endl;
        break;
      case 0:  // timeout
        // std::cout << "timeout" << std::endl;
        break;
      default:
        recv_len = ::read(fd_, data, len);
        break;
    }

    return recv_len;
  }

    void recv(uint8_t* data, uint8_t head, ssize_t len) {
      // 如果 len 非法（负数或太大），直接返回
      if (len <= 0 || static_cast<size_t>(len) > recv_buf.size()) return;

      // 接收数据放入缓冲区
      ssize_t recv_len = this->recv(recv_buf.data(), len);
      for (ssize_t i = 0; i < recv_len; i++) {
          recv_queue.push(recv_buf[i]);
      }

      // 查找帧头
      while (recv_queue.size() >= static_cast<size_t>(len)) {
          if (recv_queue.front() != head) {
              recv_queue.pop();
              continue;
          }
          break;
      }

      if (recv_queue.size() < static_cast<size_t>(len)) return;

      // 读取数据
      for (ssize_t i = 0; i < len; i++) {
          data[i] = recv_queue.front();
          recv_queue.pop();
      }
  }


  void set_timeout(int timeout_ms) {
    timeout_.tv_sec = timeout_ms / 1000;
    timeout_.tv_usec = (timeout_ms % 1000) * 1000;
  }

 private:
  int Init(std::string port, speed_t baudrate) {
    // Open serial port
    fd_ = open(port.c_str(), O_RDWR | O_NOCTTY);
    if (fd_ < 0) {
      printf("Open serial port %s failed\n", port.c_str());
      exit(-1);
    }

    // Set attributes
    struct termios option;
    memset(&option, 0, sizeof(option));
    int ret = tcgetattr(fd_, &option);

    option.c_oflag = 0;
    option.c_lflag = 0;
    option.c_iflag = 0;

    cfsetispeed(&option, baudrate);
    cfsetospeed(&option, baudrate);

    option.c_cflag &= ~CSIZE;
    option.c_cflag |= CS8;      // 8
    option.c_cflag &= ~PARENB;  // no parity
    option.c_iflag &= ~INPCK;   // no parity
    option.c_cflag &= ~CSTOPB;  // 1 stop bit

    option.c_cc[VTIME] = 0;
    option.c_cc[VMIN] = 0;
    option.c_lflag |= CBAUDEX;

    ret = tcflush(fd_, TCIFLUSH);
    ret = tcsetattr(fd_, TCSANOW, &option);

      return ret;
  }

  int fd_;
  fd_set rSet_;
  timeval timeout_;

  std::queue<uint8_t> recv_queue;
  std::array<uint8_t, 1024> recv_buf;
};

#endif  // SERIAL_PORT_H